#include <ros/ros.h>
#include "quad_msgs/ThrustAndAttitude.h"
#include <dji_osdk_ros/FlightTaskControl.h>
#include <dji_osdk_ros/ObtainControlAuthority.h>
#include <geometry_msgs/Vector3Stamped.h>

using namespace dji_osdk_ros;

void djiAccelerationCallback(const geometry_msgs::Vector3Stamped &msg)
{
    ROS_INFO("Acce: (%f, %f, %f)", msg.vector.x, msg.vector.y, msg.vector.z);
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "dji_controller");
    ros::NodeHandle nh;

    auto obtain_ctrl_authority_client = nh.serviceClient<dji_osdk_ros::ObtainControlAuthority>("obtain_release_control_authority");
    ObtainControlAuthority obtainCtrlAuthority;
    obtainCtrlAuthority.request.enable_obtain = true;
    obtain_ctrl_authority_client.call(obtainCtrlAuthority);

    ros::ServiceClient task_control_client = nh.serviceClient<FlightTaskControl>("/flight_task_control");
    FlightTaskControl control_task;
    control_task.request.task = FlightTaskControl::Request::TASK_TAKEOFF;
    ROS_INFO_STREAM("Takeoff request sending ...");
    task_control_client.call(control_task);
    if (control_task.response.result == false)
    {
        ROS_ERROR_STREAM("Takeoff task failed");
        return 0;
    }
    ROS_INFO_STREAM("Takeoff task successful");
    ros::Duration(5.0).sleep();

    auto dji_acceleration_sub_ = nh.subscribe("/dji_osdk_ros/acceleration_ground_fused", 1,
                                              djiAccelerationCallback);

    ros::Publisher control_command_pub_ = nh.advertise<quad_msgs::ThrustAndAttitude>(
        "/dji_sdk/thrust_control", 10);

    double force;
    nh.param("calib_force/force", force, 20.0);

    ros::Rate rate(10);
    quad_msgs::ThrustAndAttitude msg;
    msg.thrust = force;
    // msg.roll = 15; // NED坐标系+y 即东边
    // msg.pitch = 15; // NED坐标系-x 即南边

    // msg.yaw = 45;

    // for (size_t i = 0; i < 30; i++)
    // {
    //     control_command_pub_.publish(msg);
    //     ros::spinOnce();
    //     rate.sleep();
    // }
    

    // msg.pitch = 7;

    for (size_t i = 0; i < 50; i++)
    {
        control_command_pub_.publish(msg);
        ros::spinOnce();
        rate.sleep();
    }

    // ros::spin();
    return 0;
}
